#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include <fstream>
#include <cmath>
#include <string>

#define rad 57.29578

using namespace std;
using namespace cv;

class Kernel{
	public:
		int cols,rows;
		int **ele;
		Kernel(int x, int y){
			rows=x;
			cols=y;
			ele = new int *[rows];
			for(int i=0;i<rows;i++){
				ele[i]=new int[cols];
			}
			for(int i=0;i<rows;i++){
				for(int j=0;j<cols;j++){
					ele[i][j]=0;
				}
			}
		}
		void set_ele(int **k){
			for(int i=0;i<rows;i++){
				for(int j=0;j<cols;j++){
					ele[i][j]=k[i][j];
				}
			}
		}
};

class comp{
	public:
		int x;
		comp* next;
		comp(int num){
			x=num;
			next=NULL;
		}
};

class eq_class_head{
	private:
		void insert(comp *p){
			if(start==NULL || start->x>p->x){
				p->next=start;
				start=p;
				return;
			}
			else{
				comp *ptr=start;
				while(ptr!=NULL){
					if(ptr->next==NULL || ptr->next->x>p->x){
						p->next=ptr->next;
						ptr->next=p;
						return;
					}
					ptr=ptr->next;
				}
			}
		}
	public:
		comp* start;
		int count,x_l,x_r,y_u,y_d;
		eq_class_head* next;
		eq_class_head(){
			start=NULL;
			next=NULL;
			count=x_r=y_d=0;
			x_l=y_u=2047483647;
		}
		void eq_class_insert(int num){
			comp *p = new comp(num);
			insert(p);
		}
};


void pixel_set(Mat *, int, int ,int);
uchar pixel_get(Mat *, int ,int);
void pixel_swap(Mat *, int , int , int , int);
Mat binary_image(char *,int);
int** binary_image_arr(Mat *, int);
void classify(Mat *, int **);
void eq_class(int, int);
void eq_class(int);
void show_eq_class();
void eq_count(Mat *, int **);
Mat3b block_comp(Mat *,char *,char *, int);
Mat * erosion(Mat *);
uchar ero_ele(Mat *, Kernel *, int , int ); 
Mat * dilation(Mat *);
uchar di_ele(Mat *, Kernel *, int , int );

eq_class_head *eq_table=NULL;     //equivilent table


int main(int argc, char **argv){
	// Check number of arguments.
	if ( argc != 2 )
		exit(1);

	Mat *SrcImage;
	Mat binImage;
	Mat3b WorkImage;

	// Load a gray scale picture.
	SrcImage = new Mat;
	*SrcImage = imread(argv[1],0);
	if (SrcImage == NULL)
		exit(1);

	// Create windows for debug.
	namedWindow("SrcImage", CV_WINDOW_AUTOSIZE);
	namedWindow("BinaryImage", CV_WINDOW_AUTOSIZE);
	namedWindow("WorkImage", CV_WINDOW_AUTOSIZE);

	// Show the source image.
	imshow("SrcImage", *SrcImage);	
	cout << "haha"  << endl;
	// Duplicate the source iamge.
	//WorkImage = new Mat;
	//*WorkImage = SrcImage->clone();

	/* If you're familiar with OpenCV, cvFindContours() will be a better way.*/
	binImage = binary_image(argv[1],128);
	imshow("BinaryImage", binImage);
	Mat *ComImage = new Mat;
	*ComImage = SrcImage->clone();
	ComImage = erosion(ComImage);
	ComImage = dilation(ComImage);
	int **arr=binary_image_arr(ComImage,128);
    classify(ComImage, arr);
	eq_count(ComImage, arr);
	string name = argv[1];
	name.replace(name.size()-4, 4, "_result.jpg");
	WorkImage = block_comp(ComImage,argv[1],(char *)name.c_str(),128);
	imshow("WorkImage", WorkImage);	
	//end of work

	waitKey();
	return 0;
}




void pixel_set(Mat *p, int x, int y, int value){	//set B_PIX(p,x,y) to 'value'
	uchar* tp = p->data+x*p->cols+y;
	*tp = value;
}

uchar pixel_get(Mat *p, int x,int y){			//return B_PIX(p,x,y)
	if(x>=p->rows || x<0 || y>=p->cols || y<0)
		return 0;
	uchar* tp = p->data+x*p->cols+y;
	return *tp;
}

void pixel_swap(Mat *p, int x, int y, int xp, int yp){	//swap B_PIX(p,x,y) and B_PIX(p,xp,yp)
	uchar pix = pixel_get(p,xp,yp);
	pixel_set(p,xp,yp,pixel_get(p,x,y));
	pixel_set(p,x,y,pix);
}

Mat binary_image(char *input, int treshold){
	Mat image=imread(input,0);
	for(int i=0;i<image.rows;i++){
		for(int j=0;j<image.cols;j++){
			if((int)pixel_get(&image,i,j)>=128){
				pixel_set(&image,i,j,255);
			}
			else{
				pixel_set(&image,i,j,0);
			}
		}
	}
	return image;
}

int** binary_image_arr(Mat *input, int treshold){
    Mat image=input->clone();
    int **arr = new int*[image.rows];
    for(int i=0;i<image.rows;i++){
        arr[i]=new int[image.cols];
    }
    for(int i=0;i<image.rows;i++){
        for(int j=0;j<image.cols;j++){
            if((int)pixel_get(&image,i,j)>=128){
                arr[i][j]=1;
            }
            else{
                arr[i][j]=0;
            }
        }
    }
    return arr;
}

void classify(Mat *input, int **arr){
	Mat image=input->clone();
	int row=image.rows;
	int col=image.cols;
	int comp=0; // components
	for(int i=0;i<row;i++){
		for(int j=0;j<col;j++){
			if(arr[i][j]>0){ //a component
				if(i-1>=0 && arr[i-1][j]>0){  //up pixel is a component	
					arr[i][j]=arr[i-1][j];
					if(j-1>=0 && arr[i][j-1]>0 && arr[i][j-1]!=arr[i-1][j]){
						//cout << "eq_class " << arr[i][j] << " " << arr[i][j-1] << endl;
						eq_class(arr[i][j],arr[i][j-1]);//eq_class(arr[i][j],arr[i][j-1]); //make x and y same class
					}
				}
				else if(j-1>=0 && arr[i][j-1]>0){
					arr[i][j]=arr[i][j-1];
				}
				else{
					arr[i][j]=++comp;
					eq_class(arr[i][j]);//eq_class(arr[i][j]) create a new class
				}
			}
		}
	}
}

void eq_class(int x, int y){
	eq_class_head *p=eq_table;	
	eq_class_head *x_locate=NULL, *y_locate=NULL;
	while(p!=NULL){
		comp *ele=p->start;		
		while(ele!=NULL){
			if(ele->x==x){
				x_locate=p;
				break;
			}
			else if(ele->x==y){
				y_locate=p;
				break;
			}
			ele=ele->next;
		}
		if(x_locate!=NULL && y_locate!=NULL){
			break;
		}
		p=p->next;
	}

	if(x_locate==NULL || y_locate==NULL){
		return;
	}
	comp *y_num=y_locate->start;
	while(y_num!=NULL){
		x_locate->eq_class_insert(y_num->x);
		comp *temp=y_num;
		y_num=y_num->next;
		delete temp;
	}
		
	//delete class y;
	if(y_locate==eq_table){
		eq_table=eq_table->next;
		delete y_locate;
	}
	else{
		eq_class_head *ptr=eq_table;
		while(ptr!=NULL){
			if(ptr->next==y_locate){
				ptr->next=y_locate->next;
				delete y_locate;
				break;
			}
			ptr=ptr->next;
		}
	}
}

void eq_class(int x){
	eq_class_head *p=new eq_class_head;
	p->eq_class_insert(x);
	eq_class_head *ptr=eq_table;
	if(ptr==NULL){
		eq_table=p;
	}
	else{
		while(ptr->next!=NULL){
			ptr=ptr->next;
		}
		ptr->next=p;
	}
}

void show_eq_class(){
	eq_class_head *p=eq_table;
	while(p!=NULL){
		if(p->count>=500){
			comp *c=p->start;
			while(c!=NULL){
				cout << c->x << " ";
				c=c->next;
			}
			cout << "count: " << p->count << endl;
			cout << "x_l: " << p->x_l << "x_r: " << p->x_r << "y_u: " << p->y_u << "y_d: " << p->y_d << endl;
 		}
 		p=p->next;
	}
}

void eq_count(Mat *input, int **arr){
	Mat image=input->clone();
	int row=image.rows;
	int col=image.cols;
	eq_class_head *p=eq_table;
	while(p!=NULL){
		comp *ele=p->start;
		int l=ele->x;
		while(ele!=NULL){
			for(int i=0;i<row;i++){
				for(int j=0;j<col;j++){
					if(arr[i][j]==ele->x){
						arr[i][j]=l;
						p->count++;
						if(p->x_l>i){
							p->x_l=i;
						}
						if(p->x_r<i){
							p->x_r=i;
						}
						if(p->y_u>j){
							p->y_u=j;
						}
						if(p->y_d<j){
							p->y_d=j;
						}
					}
				}
			}
			ele=ele->next;
		}
		p=p->next;
	}
	
}

Mat3b block_comp(Mat *gg_image ,char *input,char *output, int treshold){
	Mat3b image=imread(input);
	//cout << image.rows << " " << image.cols << endl;
	eq_class_head *p=eq_table;
	while(p!=NULL){
		if(p->count>=500){
			int x_w=(p->x_l+p->x_r)/2;
			int y_w=(p->y_u+p->y_d)/2;
			//cout << "centroid: (" << x_w << "," << y_w << ")" << endl;
			int x_c, y_c;
			int m_00,m_10,m_01;
			x_c=y_c=m_00=m_10=m_01=0;
			Mat g_image=imread(input,0);
			
			//adj

			for(int i=p->x_l;i<=p->x_r;i++){
				for(int j=p->y_u;j<=p->y_d;j++){
					if((int)pixel_get(gg_image,i,j)>=128){
						if((int)pixel_get(gg_image,i-1,j)<128 ||(int)pixel_get(gg_image,i+1,j)<128 ||(int)pixel_get(gg_image,i,j-1)<128 || (int)pixel_get(gg_image,i,j+1)<128 ){
							for(int ii=-5;ii<5;ii++){
								for(int jj=-5;jj<5;jj++){
									if( sqrt(float(ii*ii+jj*jj)) <=2){
										if(i+ii<image.rows && i+ii >=0 && j+jj<image.cols && j+jj >=0){
											image.at<cv::Vec3b>(i+ii,j+jj)[0] = 200;
											image.at<cv::Vec3b>(i+ii,j+jj)[1] = 100;
											image.at<cv::Vec3b>(i+ii,j+jj)[2] = 0;
										}
									}
								}
							}
						}
					}
				}
			}
			for(int i=p->x_l;i<p->x_r;i++){
				for(int j=p->y_u;j<p->y_d;j++){
					if((int)pixel_get(&g_image,i,j)>=128){
						m_00++;
						m_10+=i;
						m_01+=j;
					}
				}
			}
			x_c=m_10/m_00;
			y_c=m_01/m_00;
			cout << "Centroid = (" << y_c << ", " << x_c << ")" << endl;
			for(int i=x_c-5;i<=x_c+5;i++){
				image.at<cv::Vec3b>(i,y_c)[0] = 0;
				image.at<cv::Vec3b>(i,y_c)[1] = 0;
				image.at<cv::Vec3b>(i,y_c)[2] = 255;
			}
			for(int i=y_c-5;i<=y_c+5;i++){
				image.at<cv::Vec3b>(x_c,i)[0] = 0;
				image.at<cv::Vec3b>(x_c,i)[1] = 0;
				image.at<cv::Vec3b>(x_c,i)[2] = 255;
			}
			double mu_11,mu_20,mu_02;
			mu_11=mu_20=mu_02=0;
			for(int i=p->x_l;i<p->x_r;i++){
				for(int j=p->y_u;j<p->y_d;j++){
					if((int)pixel_get(&g_image,i,j)>=128){
						mu_11+=((i-x_c)*(j-y_c));
						mu_20+=((j-y_c)*(j-y_c));
						mu_02+=((i-x_c)*(i-x_c));
					}
				}
			}
			//cout<<"mu11:"<<mu_11<<" mu20:"<<mu_20<<" mu02:"<<mu_02<<endl;
			double theta = atan2(2*mu_11,mu_20-mu_02)/2;
			double ratio=tan(theta);
			//cout << "tan(fy): " << ratio << endl;
			double principal_angle=theta*rad;
			cout<<"principal angle: " << principal_angle << " degree"<<endl<<endl;
			for(int i=x_c;i<p->x_r;i++){
				if((y_c+(i-x_c)/ratio)>=p->y_u && (y_c+(i-x_c)/ratio)<p->y_d){
					image.at<Vec3b>(i,y_c+(i-x_c)/ratio)[0]=0;
					image.at<Vec3b>(i,y_c+(i-x_c)/ratio)[1]=255;
					image.at<Vec3b>(i,y_c+(i-x_c)/ratio)[2]=0;
				}
			}

			for(int i=p->y_u;i<p->y_d;i++){
				if( (x_c+(i-y_c)*ratio)>=p->x_l  && (x_c+(i-y_c)*ratio)<p->x_r ){
					image.at<Vec3b>(x_c+(i-y_c)*ratio,i)[0]=0;
					image.at<Vec3b>(x_c+(i-y_c)*ratio,i)[1]=255;
					image.at<Vec3b>(x_c+(i-y_c)*ratio,i)[2]=0;
				}
			}

		}
		p=p->next;
	}
	imwrite(output,image);
	return image;	
}

Mat * erosion(Mat *p){
	//kerker
	int oct[5][5]={0,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,0};
	int **r=new int*[5];
	for(int i=0;i<5;i++){
		r[i]=new int[5];
	}
	for(int i=0;i<5;i++){
		for(int j=0;j<5;j++){
			r[i][j]=oct[i][j];
		}
	}
	Kernel k(5,5);
	k.set_ele(r);
	Mat *result=new Mat(p->rows,p->cols,0);
	for(int i=0;i<p->rows;i++){
		for(int j=0;j<p->cols;j++){
			pixel_set(result,i,j,0);
		}
	}
	for(int i=0;i<p->rows;i++){
		for(int j=0;j<p->cols;j++){
			pixel_set(result,i,j,(int)ero_ele(p,&k,i,j));
		}
	}
	return result;
}

uchar ero_ele(Mat *p, Kernel *k, int x, int y){
	int p_x=x-(k->rows/2);
	int p_y=y-(k->cols/2);
	uchar num=pixel_get(p,x,y);
	for(int i=0;i<k->rows;i++){
		for(int j=0;j<k->cols;j++){
			if(p_x+i>=0 && p_x+i<p->rows && p_y+j>=0 && p_y+j<p->cols && k->ele[i][j]==1){
				num=(uchar)min((int)num, (int)pixel_get(p,p_x+i,p_y+j));
			}
		}
	}
	return num;
}


Mat * dilation(Mat *p){
	//kerker
	int oct[5][5]={0,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,0};
	int **r=new int*[5];
	for(int i=0;i<5;i++){
		r[i]=new int[5];
	}
	for(int i=0;i<5;i++){
		for(int j=0;j<5;j++){
			r[i][j]=oct[i][j];
		}
	}
	Kernel k(5,5);
	k.set_ele(r);

	Mat *result=new Mat(p->rows,p->cols,0);
	for(int i=0;i<p->rows;i++){
		for(int j=0;j<p->cols;j++){
			pixel_set(result,i,j,0);
		}
	}
	for(int i=0;i<p->rows;i++){
		for(int j=0;j<p->cols;j++){
			pixel_set(result,i,j,(int)di_ele(p,&k,i,j));
		}
	}
	return result;
}

uchar di_ele(Mat *p, Kernel *k, int x, int y){
	int p_x=x-(k->rows/2);
	int p_y=y-(k->cols/2);
	uchar num=pixel_get(p,x,y);
	for(int i=0;i<k->rows;i++){
		for(int j=0;j<k->cols;j++){
			if(p_x+i>=0 && p_x+i<p->rows && p_y+j>=0 && p_y+j<p->cols && k->ele[i][j]==1){
				num=(uchar)max((int)num, (int)pixel_get(p,p_x+i,p_y+j));
			}
		}
	}
	return num;
}
